#include "camera.h"
#include "stdint.h"
#include "GlobalData.h"
#include "LCD.h"
#include <stdlib.h>
#include "math.h"
#include "stdio.h"
#include "Process.h"
#include "CAN.h"
#include "CMD.h"

const uint8_t actanTable[100] = 
{
    0,  1,  1,  2,  2,  3,  3,  4,  5,  5,
    6,  6,  7,  7,  8,  9,  9,  10, 10, 11,
    11, 12, 12, 13, 13, 14, 15, 15, 16, 16,
    17, 17, 18, 18, 19, 19, 20, 20, 21, 21,
    22, 22, 23, 23, 24, 24, 25, 25, 26, 26,
    27, 27, 27, 28, 28, 29, 29, 30, 30, 31,
    31, 31, 32, 32, 33, 33, 33, 34, 34, 35,
    35, 35, 36, 36, 37, 37, 37, 38, 38, 38,
    39, 39, 39, 40, 40, 40, 41, 41, 41, 42, 
    42, 42, 43, 43, 43, 44, 44, 44, 44, 45
};

//排序
void sort(uint8_t *a, uint8_t len)//a为数组地址，l为数组长度。
{
    int i, j;
    int v;
    //排序主体
    for(i = 0; i < len - 1; i ++)
    {
        for(j = i+1; j < len; j ++)
        {
            if(a[i] > a[j])//如前面的比后面的大，则交换。
            {
                v = a[i];
                a[i] = a[j];
                a[j] = v;
            }
        }
    }
}
/*
* 发送二值化数据到PC
*/
void ImageToPC(void)
{
    uint8_t i,j;
    printf("Start!\r\n");
    for (i = 0; i < 240; i ++)
    {
        for (j = 0; j < 160; j ++)
        {
            printf("%d ", imageGrayBuf[i][j]);
        }
    }
    printf("End!\r\n");
}

/*
* @func GetIterativeBestThreshold
*
* @param[in] none
* 
* @return none
*
* @brief 迭代最佳阈值
*/
int GetIterativeBestThreshold(void)
{
    int X, Iter = 0;
    int MeanValueOne, MeanValueTwo, SumOne, SumTwo, SumIntegralOne, SumIntegralTwo;
    int MinValue, MaxValue;
    int Threshold, NewThreshold;

    for (MinValue = 0; MinValue < 256 && imageHist[MinValue] == 0; MinValue++);
    for (MaxValue = 255; MaxValue > MinValue && imageHist[MinValue] == 0; MaxValue--) ;

    if (MaxValue == MinValue) 
        return MaxValue;          // 图像中只有一个颜色             
    if (MinValue + 1 == MaxValue) 
        return MinValue;      // 图像中只有二个颜色

    Threshold = MinValue;
    NewThreshold = (MaxValue + MinValue) >> 1;
    while (Threshold != NewThreshold)    // 当前后两次迭代的获得阈值相同时，结束迭代
    {
        SumOne = 0; SumIntegralOne = 0;
        SumTwo = 0; SumIntegralTwo = 0;
        Threshold = NewThreshold;
        for (X = MinValue; X <= Threshold; X++)         //根据阈值将图像分割成目标和背景两部分，求出两部分的平均灰度值      
        {
            SumIntegralOne += imageHist[X] * X;
            SumOne += imageHist[X];
        }
        MeanValueOne = SumIntegralOne / SumOne;
        for (X = Threshold + 1; X <= MaxValue; X++)
        {
            SumIntegralTwo += imageHist[X] * X;
            SumTwo += imageHist[X];
        }
        MeanValueTwo = SumIntegralTwo / SumTwo;
        NewThreshold = (MeanValueOne + MeanValueTwo) >> 1;       //求出新的阈值
        Iter++;
        if (Iter >= 1000) 
        return -1;
    }
    return Threshold;
}

/*
* @func GetImageHistFrontHalf
*
* @param[in] none
* 
* @return none
*
* @brief 获得前一半图像的灰度直方图
*/
void GetImageHistFrontHalf(void)
{
    uint16_t i, j;
		uint16_t *p;
    uint16_t R, G, B;
    //uint8_t temp;
    uint8_t Y, U, V;
    /*
    ** 初始化直方图数组
    */
    for (i = 0; i < 256; i++)
    {
        imageHist[i] = 0;
    }
    p = cameraBuf;
    /*
    ** 将 RGB565 按照比例转为 RGB888，然后得到灰度图像
    ** RGB 转为 YUV公式：
    ** Y = 0.299*R + 0.587*G + 0.114*B; //灰度
    ** U = -0.147*R - 0.289*G + 0.436*B;
    ** V = 0.615*R - 0.515*G - 0.100*B;
    ** 为了提高效率，避免浮点运算
    */
	for (i = 0; i < 120; i ++)
    {
        for (j = 0; j < 160; j ++)
        {
            R = ((p[i*160 +j]>>11) & 0x001F) * 255/63;
            G = ((p[i*160 +j]>>5) & 0x003F) * 255/127;
            B = ((p[i*160 +j]>>0) & 0x001F) * 255/63;
            /*
            ** RGB分量图像和灰度图像类似，可以不做如下处理，直接用某一分量代替灰度图
            */
            Y = (uint8_t)(0.299f*R + 0.587f*G + 0.114f*B);
            imageGrayBuf[i][j] = Y;
            /*
            ** 由灰度图像得到直方图
            */
            imageHist[Y] ++;
        }
    }
}
/*
* @func GetImageHist
*
* @param[in] none
* 
* @return none
*
* @brief 获得后一半图像的灰度直方图,并与前一部分合并
*/
void GetImageHistRearHalf(void)
{
    uint16_t i, j;
	uint16_t *p;
    uint16_t R, G, B;
    //uint8_t temp;
    uint8_t Y, U, V;
    p = cameraBuf;
    /*
    ** 将 RGB565 按照比例转为 RGB888，然后得到灰度图像
    ** RGB 转为 YUV公式：
    ** Y = 0.299*R + 0.587*G + 0.114*B; //灰度
    ** U = -0.147*R - 0.289*G + 0.436*B;
    ** V = 0.615*R - 0.515*G - 0.100*B;
    ** 为了提高效率，避免浮点运算
    */
	for (i = 120; i < 240; i ++)
    {
        for (j = 0; j < 160; j ++)
        {
            R = ((p[i*160 +j]>>11) & 0x001F) * 255/63;
            G = ((p[i*160 +j]>>5) & 0x003F) * 255/127;
            B = ((p[i*160 +j]>>0) & 0x001F) * 255/63;
            Y = (uint8_t)(0.299f*R + 0.587f*G + 0.114f*B);
            imageGrayBuf[i][j] = Y;
            /*
            ** 由灰度图像得到直方图
            */
            imageHist[Y] ++;
        }
    }
}
/*
* @func GetLeadLineLocation
*
* @param[in] none
* 
* @return none
*
* @brief 获取引导线位置
*/
void GetLeadLineLocation(void)
{
    uint16_t i, j;
    uint8_t x0, y0, firstPoint[21];
    uint8_t x1, y1, secondPoint[21];
    //float tanValue;
    
    /*
    ** 初始化阈值
    */
    threGray = 0;
    /*
    ** 获得动态二值化阈值
    */
    threGray = GetIterativeBestThreshold();
    /*
    ** 灰度图像二值化
    */
    for (i = 0; i < 240; i ++)
    {
        for (j = 0; j < 160; j ++)
        {
            if (imageGrayBuf[i][j] >= threGray)
            {
                imageGrayBuf[i][j] = 255; /* 背景用白色表示 */
            }
            else
            {
                imageGrayBuf[i][j] = 0; /* 引导线用黑色表示 */
            }
        }
        /*
        ** 提取引导线左边界
        */
        for (j = 0; j < 160 - 1; j ++)
        {
            if ((imageGrayBuf[i][j] - imageGrayBuf[i][j+1]) == 255)// || 
                //(imageGrayBuf[i][j] - imageGrayBuf[i][j+1]) == -255)
            {
                leadLine[i] = j + 1;
                break;
            }
            else
            {
                leadLine[i] = 0;
            }
        }
    }
    /*
    ** 获取边界线两端点 (x0, y0)、(x1, y1)
    */
    for (i = 0; i < 21; i ++)
    {
        firstPoint[i] = leadLine[i];
        secondPoint[i] = leadLine[239 - i];
    }
    /*
    ** 排序，中值滤波
    */
    sort(firstPoint, 21);
    sort(secondPoint, 21);
    x0 = firstPoint[10];
    x1 = secondPoint[10];
    y0 = 10;
    y1 = 229;
    //tanValue = (float)(x1 - x0) / (float)(y1 - y0);
    //横向扫描置零
    lineLocation[CAMERA_INDEX].txdata[0] = x0;
    lineLocation[CAMERA_INDEX].txdata[1] = x1;
    //纵向扫描信息有效
    lineLocation[CAMERA_INDEX].txdata[2] = 0;
    lineLocation[CAMERA_INDEX].txdata[3] = 0;
    //lineLocation[CAMERA_INDEX].txdata[1] = x1;  //边沿线末端点位置信息
//    //取tanValue的小数点后两位，查表得到角度
//    if (tanValue >= 0)
//    {
//        lineLocation[CAMERA_INDEX].txdata[2] = 0;  //角度正号
//        lineLocation[CAMERA_INDEX].txdata[3] = actanTable[(int8_t)(tanValue * 100)];  //边沿线角度信息 0 ~ 45°
//    }
//    else
//    {
//        lineLocation[CAMERA_INDEX].txdata[2] = 1;  //角度负号
//        lineLocation[CAMERA_INDEX].txdata[3] = actanTable[(-1) * (int8_t)(tanValue * 100)];  //边沿线角度信息 0 ~ 45°
//    }
}

/*
 * SearchLeadLine
 */
void SearchLeadLine(void)
{
    uint16_t i, j;
    uint8_t x0, y0, firstPoint[21];
    uint8_t x1, y1, secondPoint[21];
    /*
    ** 初始化阈值
    */
    threGray = 0;
    /*
    ** 获得动态二值化阈值
    */
    threGray = GetIterativeBestThreshold();
    /*
    ** 灰度图像二值化
    */
    for (j = 0; j < 160; j ++)
    {
        for (i = 0; i < 240; i ++)
        {
            if (imageGrayBuf[i][j] >= threGray)
            {
                imageGrayBuf[i][j] = 255; /* 背景用白色表示 */
            }
            else
            {
                imageGrayBuf[i][j] = 0; /* 引导线用黑色表示 */
            }
        }
        /*
        ** 提取引导线上边界
        */
        for (i = 1; i < 240 - 1; i ++)//这里从零开始扫描会出问题
        {
            if ((imageGrayBuf[i][j] - imageGrayBuf[i+1][j]) == 255)// || 
                //(imageGrayBuf[i][j] - imageGrayBuf[i+1][j]) == -255)
            {
                leadLine2[j] = i + 1;
                break;
            }
            else
            {
                leadLine2[j] = 0;
            }
        }
    }
    /*
    ** 获取边界线两端点 (x0, y0)、(x1, y1)
    */
    for (i = 0; i < 21; i ++)
    {
        firstPoint[i] = leadLine2[i];
        secondPoint[i] = leadLine2[159 - i];
    }
    /*
    ** 排序，中值滤波
    */
    sort(firstPoint, 21);
    sort(secondPoint, 21);
    x0 = firstPoint[10];
    x1 = secondPoint[10];
    //y0 = 10;
    //y1 = 229;
    //横向扫描置零
    lineLocation[CAMERA_INDEX].txdata[0] = 0;
    lineLocation[CAMERA_INDEX].txdata[1] = 0;
    //纵向扫描信息有效
    lineLocation[CAMERA_INDEX].txdata[2] = x0;
    lineLocation[CAMERA_INDEX].txdata[3] = x1;
}


/*
* @func ImageShowLCD
*
* @param[in] none
* 
* @return none
*
* @brief 在LCD上显示图像
*/

void ImageShowLCD(uint8_t type)
{
    uint32_t i;
    uint8_t j;
    uint16_t *P;
    P = cameraBuf;
    /*
    ** 显示彩色图像
    */
    if (type == RGB_TYPE) 
    {
        for(i = 0; i < 240*160; i ++)
        {
            LCD_WriteData(P[i]);
        }
    }
    /*
    ** 显示二值化图像
    */
    else if (type == BINARY_TYPE)
    {
        for(i = 0; i < 240; i++)
        {
            for(j = 0; j < 160; j++)
            {
                /*
                ** 未加入显示引导线中心线
                */
                if ((imageGrayBuf[i][j]) == 255)
                {
                    LCD_WriteData(White); /* 引导线白色表示 */
                }
                else
                {
                    LCD_WriteData(Black); /* 背景黑色表示 */
                }
            }
        }
    }
}

/*
* @func ImageProces
*
* @param[in] none
* 
* @return none
*
* @brief 图像处理算法,基于灰度直方图的引导线位置提取
*/
void ImageProcess(void)
{
    /*
    ** 读取模式信息
    */
    robotStatus.mode = mainInfo[0].rxdata[0];
    if ((robotStatus.mode == SEARCHLINE) || (robotStatus.mode == LEAD))  //只有在寻线模式、引导模式下需要处理图像
    {
        /*
        ** 计算前半部分图像灰度直方图
        */
        if (flagManager.frontHalfNeedProcess == 1)
        {
            flagManager.frontHalfNeedProcess = 0; //清标志位
            GetImageHistFrontHalf();//6 ms
        }
        /*
        ** 计算后半部分图像灰度直方图，合并前一半图像灰度直方图
        */
        if (flagManager.rearHalfNeedProcess == 1)
        {
            flagManager.rearHalfNeedProcess = 0; //清标志位
            GetImageHistRearHalf(); //6 ms 
            flagManager.grayHistReady = 1;
        }
        /*
        ** 根据灰度直方图，获取引导线位置
        */
        if (flagManager.grayHistReady == 1)
        {
            flagManager.grayHistReady = 0; //清标志位

            if (robotStatus.mode == SEARCHLINE)
            {
                //搜索引导线
                SearchLeadLine();
            }
            else
            {
                //获得引导线位置
                GetLeadLineLocation();
            }
            /*
            ** CAN1 发送引导线位置信息
            */
            SendLineLocationCAN1();

            /*
            ** LCD 显示图像
            */
            //ImageShowLCD(BINARY_TYPE);
            /*
            ** 显示引导线位置
            */
            ShowLeadLine();
        }
    }
}

/*
* @func ShowLeadLine
*
* @param[in] none
* 
* @return none
*
* @brief 在LCD上显示引导线位置
*/
void ShowLeadLine(void)
{
	uint8_t i, j;
	/*
    ** 清屏
    */
    LCD_Clear(Blue2);
    /*
    ** 显示引导线位置
    */
    if (robotStatus.mode == LEAD) //水平扫描
    {
        for (i = 0; i < 240; i ++)
        {
            LCD_SetPoint(leadLine[i], i, Red);
        }
    }
    else if (robotStatus.mode == SEARCHLINE) //垂直扫描
    {
        for (j = 0; j < 160; j ++)
        {
            LCD_SetPoint(j, leadLine2[j], Red);
        }
    }
}


/*
* @func SendLineLocationCAN1
*
* @param[in] none
* 
* @return none
*
* @brief 通过CAN总线发送引导线位置
*/
void SendLineLocationCAN1(void)
{
    Can1WriteData(lineLocation[CAMERA_INDEX].ID, 
                  lineLocation[CAMERA_INDEX].isextend, 
                  lineLocation[CAMERA_INDEX].isdataframe, 
                  lineLocation[CAMERA_INDEX].length, 
                  (uint8_t *)(&lineLocation[CAMERA_INDEX].txdata));
}


